#!/usr/bin/env python

# http://wiki.ros.org/dynamic_reconfigure/Tutorials/HowToWriteYourFirstCfgFile
# gen.add(name, type, level, description, default, min, max)
# gen.generate(package, node_name, header_prefix)

# Xbox channel mapping
#
#     Axis description
# 0   left X (left/right)
# 1   left Y (up/down)
# 2   LT
# 3   right X
# 4   right Y
# 5   RT
# 6   pseudo trigger axis (LT-RT; pressing RT results in a positive number)
# 7   pseudo bumper axis (RB-LB; pressing RB results in a positive number)
#
#     Button description
# 0   A
# 1   B
# 2   X
# 3   Y
# 4   LB
# 5   RB
# 6   BACK
# 7   START
# 8   POWER
# 9   left stick click
# 10  right stick click
# 11  cross key left
# 12  cross key right
# 13  cross key up
# 14  cross key down

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# Xbox controller axis list
axis_enum = gen.enum([
    gen.const("Axis_Disabled",      int_t, -1, ""),
    gen.const("Left_Stick_X",       int_t, 0,  "left/right"),
    gen.const("Left_Stick_Y",       int_t, 1,  "up/down"),
    gen.const("Left_Trigger",       int_t, 2,  ""),
    gen.const("Right_Stick_X",      int_t, 3,  "left/right"),
    gen.const("Right_Stick_Y",      int_t, 4,  "up/down"),
    gen.const("Right_Trigger",      int_t, 5,  ""),
    gen.const("Trigger_Pair",       int_t, 6,  "right trigger is positive"),
], "An enum to set axis index")

# Xbox controller button list
button_enum = gen.enum([
    gen.const("Button_Disabled",    int_t, -1, ""),
    gen.const("A_Button",           int_t, 0,  ""),
    gen.const("B_Button",           int_t, 1,  ""),
    gen.const("X_Button",           int_t, 2,  ""),
    gen.const("Y_Button",           int_t, 3,  ""),
    gen.const("Left_Bumper",        int_t, 4,  ""),
    gen.const("Right_Bumper",       int_t, 5,  ""),
    gen.const("Back_Button",        int_t, 6,  ""),
    gen.const("Start_Button",       int_t, 7,  ""),
    gen.const("Power_Button",       int_t, 8,  ""),
    gen.const("Left_Stick_Click",   int_t, 9,  ""),
    gen.const("Right_Stick_Click",  int_t, 10, ""),
    gen.const("D_Pad_Left",         int_t, 11, ""),
    gen.const("D_Pad_Right",        int_t, 12, ""),
    gen.const("D_Pad_Up",           int_t, 13, ""),
    gen.const("D_Pad_Down",         int_t, 14, ""),
], "An enum to set button index")

# set axis parameters
gen.add("x_axis",           int_t, 0, "", 1,  0, 7,  edit_method=axis_enum)
gen.add("y_axis",           int_t, 0, "", 0,  0, 7,  edit_method=axis_enum)
gen.add("z_axis",           int_t, 0, "", 4,  0, 7,  edit_method=axis_enum)
gen.add("wx_axis",          int_t, 0, "", 6,  0, 7,  edit_method=axis_enum)
gen.add("wy_axis",          int_t, 0, "", -1, 0, 7,  edit_method=axis_enum)
gen.add("wz_axis",          int_t, 0, "", 3,  0, 7,  edit_method=axis_enum)

# set button parameters
gen.add("arm_button",       int_t, 0, "", 7,  0, 14, edit_method=button_enum)
gen.add("disarm_button",    int_t, 0, "", 6,  0, 14, edit_method=button_enum)
gen.add("takeoff_button",   int_t, 0, "", 0,  0, 14, edit_method=button_enum)
gen.add("land_button",      int_t, 0, "", 2,  0, 14, edit_method=button_enum)
gen.add("stabilize_button", int_t, 0, "", 1,  0, 14, edit_method=button_enum)
gen.add("alt_hold_button",  int_t, 0, "", 3,  0, 14, edit_method=button_enum)
gen.add("cam_tilt_up",      int_t, 0, "", 13, 0, 14, edit_method=button_enum)
gen.add("cam_tilt_down",    int_t, 0, "", 14, 0, 14, edit_method=button_enum)
gen.add("cam_tilt_reset",   int_t, 0, "", 9,  0, 14, edit_method=button_enum)

# set scaling parameters
linear_max = 1
angular_max = 1
gen.add("pub_rate",    double_t, 0, "the maximum publish rate", 100, 0.1, 200)
gen.add("expo",        double_t, 0, "improves control at low levels", 0.5, 0, 1)
gen.add("x_scaling",   double_t, 0, "linear scaling", 1, -linear_max, linear_max)
gen.add("y_scaling",   double_t, 0, "linear scaling", 1, -linear_max, linear_max)
gen.add("z_scaling",   double_t, 0, "linear scaling", 1, -linear_max, linear_max)
gen.add("wx_scaling",  double_t, 0, "angular scaling", 1, -angular_max, angular_max)
gen.add("wy_scaling",  double_t, 0, "angular scaling", 1, -angular_max, angular_max)
gen.add("wz_scaling",  double_t, 0, "angular scaling", -1, -angular_max, angular_max)

# tilting camera parameters
gen.add("cam_tilt_step", int_t, 0, "step size (us)", 50, 0, 1000)

# takeoff / land parameter
gen.add("takeoff_depth", double_t, 0, "depth below water surface (m)", 0.5, 0, 10)
gen.add("scend_rate",    double_t, 0, "rate of ascending/descending (m/s)", 0.5, 0, 2)

# generate the config headers
exit(gen.generate("bluerov_apps", "teleop_joy", "teleop_joy"))
